def motor_break(dly_ms: number):
    motor_disable()
    # P1 = L Motor PWM
    pins.analog_write_pin(AnalogPin.P1, 0)
    # P2 = R Motor PWM
    pins.analog_write_pin(AnalogPin.P2, 0)
    motor_enable()
    basic.pause(dly_ms)
    motor_disable()

def on_on_event():
    serial.write_line("EVT8001")
control.on_event(8000, 8001, on_on_event)

def motor_motion_ctrl():
    global stop_tmo
    if abs(accz) >= 160 or abs(accx) >= 160:
        if abs(accz) <= 160:
            motor_go_U_turn()
        else:
            motor_go_DR()
        motor_enable()
        stop_tmo = 10
def motor_disable():
    # P14: L/R Motor ENA
    pins.digital_write_pin(DigitalPin.P14, 0)
def motor_go_DR():
    global motor_tmp_pwm, motor_LR_factor, motor_R_pwm, motor_L_pwm, horn_cnt
    motor_tmp_pwm = Math.constrain(pins.map(abs(accz), 160, 600, 260, 1023), 260, 1023)
    motor_LR_factor = Math.constrain(pins.map(abs(accx), 600, 160, 0.5, 1), 0.5, 1)
    if accx < 0:
        motor_R_pwm = motor_tmp_pwm
        motor_L_pwm = motor_tmp_pwm * motor_LR_factor
    else:
        motor_L_pwm = motor_tmp_pwm
        motor_R_pwm = motor_tmp_pwm * motor_LR_factor
    if accz < 0:
        motor_run("R", "D", motor_R_pwm)
        motor_run("L", "D", motor_L_pwm)
    else:
        horn_cnt = 2
        motor_run("R", "R", motor_R_pwm)
        motor_run("L", "R", motor_L_pwm)

def on_button_pressed_a():
    global test_mode_tmo, stop_tmo
    if test_mode_tmo == 0:
        led.stop_animation()
        motor_break(5)
        test_mode_tmo = 4
        stop_tmo = 120
        motor_run("L", "D", 1023)
        motor_enable()
        basic.show_leds("""
            . # . . .
                        # # # . .
                        . # . . .
                        . # . . .
                        . # . . .
        """)
        basic.pause(3000)
        motor_break(5)
        motor_run("L", "R", 1023)
        motor_enable()
        basic.show_leds("""
            . # . . .
                        . # . . .
                        . # . . .
                        # # # . .
                        . # . . .
        """)
        basic.pause(3000)
        basic.show_leds("""
            . . . . .
                        . . . . .
                        # # # # #
                        . . . . .
                        . . . . .
        """)
        motor_break(5000)
        motor_full_stop()
input.on_button_pressed(Button.A, on_button_pressed_a)

def on_on_event2():
    serial.write_line("EVT8002")
control.on_event(8000, 8002, on_on_event2)

def motor_full_stop():
    global stop_tmo, test_mode_tmo, motor_L_predir, motor_R_predir
    motor_stop()
    # P1: L Motor PWM
    pins.analog_write_pin(AnalogPin.P1, 0)
    # P2: R Motor PWM
    pins.analog_write_pin(AnalogPin.P2, 0)
    stop_tmo = 0
    test_mode_tmo = 0
    motor_L_predir = "D"
    motor_R_predir = "D"

def on_button_pressed_b():
    global test_mode_tmo, stop_tmo
    if test_mode_tmo == 0:
        led.stop_animation()
        motor_break(5)
        test_mode_tmo = 4
        stop_tmo = 120
        motor_run("R", "D", 1023)
        motor_enable()
        basic.show_leds("""
            . . . # .
                        . . # # #
                        . . . # .
                        . . . # .
                        . . . # .
        """)
        basic.pause(3000)
        motor_break(5)
        motor_run("R", "R", 1023)
        motor_enable()
        basic.show_leds("""
            . . . # .
                        . . . # .
                        . . . # .
                        . . # # #
                        . . . # .
        """)
        basic.pause(3000)
        basic.show_leds("""
            . . . . .
                        . . . . .
                        # # # # #
                        . . . . .
                        . . . . .
        """)
        motor_break(5000)
        motor_full_stop()
input.on_button_pressed(Button.B, on_button_pressed_b)

def motor_stop():
    global accx, accz, evt6001_free
    motor_disable()
    accx = 0
    accz = 0
    evt6001_free = True

def on_received_value(name, value):
    global accz, accx, evt6001_free
    if test_mode_tmo == 0:
        if name == "accz":
            accz = value
        elif name == "accx":
            accx = value
            # until accz accx both received
            if evt6001_free:
                evt6001_free = False
                control.raise_event(6000, 6001)
radio.on_received_value(on_received_value)

def motor_enable():
    # P14: L/R Motor ENA
    pins.digital_write_pin(DigitalPin.P14, 1)

def on_logo_pressed():
    global test_mode_tmo, stop_tmo
    if test_mode_tmo == 0:
        led.stop_animation()
        motor_break(5)
        test_mode_tmo = 4
        stop_tmo = 120
        motor_run("L", "D", 1023)
        motor_run("R", "D", 1023)
        motor_enable()
        basic.show_leds("""
            . . # . .
                        . # # # .
                        # . # . #
                        . . # . .
                        . . # . .
        """)
        basic.pause(3000)
        motor_break(5)
        motor_run("L", "R", 1023)
        motor_run("R", "R", 1023)
        motor_enable()
        basic.show_leds("""
            . . # . .
                        . . # . .
                        # . # . #
                        . # # # .
                        . . # . .
        """)
        basic.pause(3000)
        basic.show_leds("""
            . . . . .
                        . . . . .
                        # # # # #
                        . . . . .
                        . . . . .
        """)
        motor_break(5000)
        motor_full_stop()
input.on_logo_event(TouchButtonEvent.PRESSED, on_logo_pressed)

def on_on_event3():
    motor_stop()
    serial.write_line("STOP by timeout cmd")
control.on_event(9000, 9001, on_on_event3)

def on_on_event4():
    motor_stop()
    serial.write_line("STOP by debounce cmd")
control.on_event(9000, 9002, on_on_event4)

def on_on_event5():
    global evt6001_free
    motor_motion_ctrl()
    evt6001_free = True
control.on_event(6000, 6001, on_on_event5)

def on_on_event6():
    global horn_cnt
    if horn_cnt != 0:
        serial.write_line("EVT8004 horn")
        horn_cnt = 1
control.on_event(8000, 8004, on_on_event6)

def on_on_event7():
    serial.write_line("EVT8003")
control.on_event(8000, 8003, on_on_event7)

def motor_go_U_turn():
    global motor_tmp_pwm
    motor_tmp_pwm = Math.constrain(pins.map(abs(accx), 160, 600, 300, 1023), 300, 1023)
    if accx < 0:
        motor_run("R", "D", motor_tmp_pwm)
        motor_run("L", "R", motor_tmp_pwm)
    else:
        motor_run("R", "R", motor_tmp_pwm)
        motor_run("L", "D", motor_tmp_pwm)
def motor_run(LR: str, DR: str, pwm: number):
    global motor_L_predir, motor_R_predir
    if LR == "L":
        if motor_L_predir != DR:
            motor_L_predir = DR
            motor_break(5)
        if DR == "D":
            # P8: L Motor Dir
            pins.digital_write_pin(DigitalPin.P8, 1)
        elif DR == "R":
            pins.digital_write_pin(DigitalPin.P8, 0)
        pins.analog_write_pin(AnalogPin.P1, pwm)
    elif LR == "R":
        if motor_R_predir != DR:
            motor_R_predir = DR
            motor_break(5)
        if DR == "D":
            # P13: R Motor Dir
            pins.digital_write_pin(DigitalPin.P13, 1)
        elif DR == "R":
            pins.digital_write_pin(DigitalPin.P13, 0)
        pins.analog_write_pin(AnalogPin.P2, pwm)
horn_cnt_local = 0
evt6001_free = False
motor_R_predir = ""
motor_L_predir = ""
test_mode_tmo = 0
motor_L_pwm = 0
motor_R_pwm = 0
motor_LR_factor = 0
motor_tmp_pwm = 0
stop_tmo = 0
accx = 0
accz = 0
horn_cnt = 0
motor_full_stop()
horn_cnt = 3
radio.set_group(180)
basic.show_string("Rx")
serial.write_line("*************************************************************")
serial.write_line("* Freda Li's Body Motion controller Rx V3.3.                *")
serial.write_line("* Nov 14,2022. 11:15                                        *")
serial.write_line("* accX accZ sent to COM every 2 seconds for debug ref      *")
serial.write_line("*************************************************************")

def on_every_interval():
    serial.write_line("z " + convert_to_text(accz) + " x " + convert_to_text(accx))
loops.every_interval(2000, on_every_interval)

def on_every_interval2():
    global horn_cnt_local, horn_cnt
    if horn_cnt > 0:
        horn_cnt_local = horn_cnt
        horn_cnt = 0
        for index in range(horn_cnt_local):
            pins.digital_write_pin(DigitalPin.P16, 0)
            basic.pause(10)
            # horn HW
            # 1 = off, 0 = on
            pins.digital_write_pin(DigitalPin.P16, 1)
            basic.pause(250)
loops.every_interval(700, on_every_interval2)

def on_every_interval3():
    if test_mode_tmo == 0:
        led.stop_animation()
        led.plot(int(pins.map(accx, -1023, 1023, 0, 5)),
            int(pins.map(accz, -1023, 1023, 0, 5)))
loops.every_interval(100, on_every_interval3)

def on_every_interval4():
    global stop_tmo
    if stop_tmo > 0:
        stop_tmo += -1
        if stop_tmo <= 0:
            motor_stop()
            serial.write_line("STOP by timeout")
loops.every_interval(100, on_every_interval4)

def on_every_interval5():
    global test_mode_tmo
    led.set_brightness(Math.constrain(pins.map(input.light_level(), 0, 200, 80, 255), 80, 255))
    if stop_tmo <= 0:
        motor_full_stop()
        serial.write_line("STOP by safety redundance")
    if test_mode_tmo > 0:
        test_mode_tmo += -1
        if test_mode_tmo <= 0:
            test_mode_tmo = 0
            motor_full_stop()
            led.stop_animation()
loops.every_interval(5000, on_every_interval5)
